#pragma once

#include <Eigen/Dense>
#include "basics/wheels.h"
#include "basics/ik_optimal_solver.h"

namespace vehicle_kinematics
{
    class VehicleKinematics2D
    {
    private:
        /* data */
        std::string center_frame_;
        std::map<std::string, VehicleWheel> wheels_;
    public:
        VehicleKinematics2D(std::string center_frame_);
        ~VehicleKinematics2D();
    public:
        bool addWheel(VehicleWheel& wheel);
    public:
        int calcFK(
            // input
            const std::vector<VehicleWheelState> wheel_states,
            // output
            Eigen::Vector2d &center_speed,
            double &center_angular
        );
        int calcIK(
            // input
            const Eigen::Vector2d &center_speed,
            const double &center_ange_speed,
            // output
            Eigen::Vector2d &instant_center_position,
            Eigen::Vector2d &instant_center_linear,
            double          &instant_center_angle_speed,
            std::vector<VehicleWheelState> wheel_states
        );

        int calcInstantIK(
            // input
            const Eigen::Vector2d &center_speed,
            const double &center_ange_speed,
            // output
            Eigen::Vector2d &instant_center_position,
            Eigen::Vector2d &instant_center_linear,
            double          &instant_center_angle_speed
        );
    };
    
    VehicleKinematics2D::VehicleKinematics(/* args */)
    {
    }
    
    VehicleKinematics2D::~VehicleKinematics2D()
    {
    }

    bool VehicleKinematics2D::addWheel(VehicleWheel& wheel)
    {
        if (wheel.getParentFrameId() != center_frame_)
        {
            return false;
        }
        wheels_[wheel.getFrameId()] = wheel;
        return true;
    }

    int VehicleKinematics2D::calcInstantIK(
                    // input
            const Eigen::Vector2d &center_speed,
            const double &center_ange_speed,
            Eigen::Vector2d &instant_center_position,
            Eigen::Vector2d &instant_center_linear,
            double          &instant_center_angle_speed
    )
    {
        instant_center_linear = {0, 0};

        // 计算瞬心位置
        instant_center_position[0] = center_speed[0] / center_ange_speed;
        instant_center_position[1] = center_speed[1] / center_ange_speed;
        instant_center_angle_speed = center_ange_speed;

        if (std::isnan(instant_center_position[0]))
        {
            instant_center_linear = center_speed;
        }

        return 0;
    }

    int VehicleKinematics2D::calcIK(
// input
            const Eigen::Vector2d &center_speed,
            const double &center_ange_speed,
// output
            Eigen::Vector2d &instant_center_position,
            Eigen::Vector2d &instant_center_linear,
            double          &instant_center_angle_speed,
            std::vector<VehicleWheelState> wheel_states
        )
        {
            wheel_states.clear();
            // 计算瞬心位置
            calcInstantIK(center_speed, center_ange_speed, 
                            instant_center_position, 
                            instant_center_linear, 
                            instant_center_angle_speed);
            // 计算每一个wheel的角速度和角度
            for(auto wheel_it : wheels_)
            {
                VehicleWheelState2D ws = wheel_it.second.calcWheelState(instant_center_position,
                                        instant_center_linear,
                                        instant_center_angular);
                wheel_states.push_back(ws);
            }
            return 0;
        }

        int VehicleKinematics2D::calcFK(
            // input
            const std::vector<VehicleWheelState> wheel_states,
            // output
            Eigen::Vector2d &center_speed,
            double &center_angular
        )
        {
            double init_vx = 0.0;
            double init_vy = 0.0;
            double init_w = 0.0;
            // 需要采用牛顿-欧拉迭代求解最优中心速度()，在此建设每一个wheel方向上的速度与wheel当前的速度方差越小，表明阻力越小
            // 求解时间限制 < 0.01s
            high_resolution_clock::time_point start_time = high_resolution_clock::now();
            int result = calcMinimumIK(inti_vx, init_vy, init_w, wheel_states, start_time)) ? 0 : 1;
            center_speed[0] = init_vx;
            center_speed[1] = init_vy;
            center_angular = init_w;
            return result;
        }
    
} // namespace vechile_kinematics
